//--- Projeto Domus
// interactionsource.com
// Koji Pereira, Andre Veloso e Marcos Paulo

/*
 Projeto Domus
 interactionsource.com
 Koji Pereira, Andre Veloso e Marcos Paulo

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

// Bola do teto

// MMA7260QT 3-Axis Accelerometer
// Sampling rate - 350Hz eixos X e Y - 1 sample a cada 2.857ms
// 150Hz eixo Z - 1 sample a cada 6.66ms

// Nessa versao: 
//Ao comecar o movimento, sortear uma cor e manter essa cor enquanto houver movimento
//Ao parar, volta ao vermelho piscante 
//no sorteio de cor, nao pode ter a cor vermelha e zero
//Utiliza a forca do impacto ocmo parametro de ajuste da temporizacao de repouso


/*

Orange  (RGB: 255, 127, 0)
Yellow  (RGB: 255, 255, 0)
Indigo  (RGB: 111, 0, 255)
Violet  (RGB: 143, 0, 255)

Red     (RGB: 255, 0,   0)
Green   (RGB: 0,   255, 0)
Blue    (RGB: 0,   0, 255)

 */

#define pinX 0   // Numero do pino onde se vai ligar o X do acelerometro
#define pinY 1   // Numero do pino onde se vai ligar o Y do acelerometro
#define pinZ 2   // Numero do pino onde se vai ligar o Z do acelerometro

int valX = 0; 
int valY = 0; 
int valZ = 0;

float tensaoX;
float tensaoY;
float tensaoZ;

float forcaX;
float forcaY;
float forcaZ;

float oldValX = 0; 
float oldValY = 0; 
float oldValZ = 0;

float diffX = 0;
float diffY = 0;
float diffZ = 0;

int LedR = 3;
int LedG = 5;
int LedB = 6;

unsigned char redVal;
unsigned char greenVal;
unsigned char blueVal;

int lastRedVal;
int lastGreenVal;
int lastBlueVal;

int red_min = 150;
int red_max = 255;

int blue_min = 185;
int blue_max = 255;

int green_min = 195;
int green_max = 255;

float h = 0;
float s = 1.0;
float v = 0.8;

float qtMovimentacao = 0;

float resultante;

float oldResultante;

float diffResultante;

float pulso = 0;
char sig_pulso = 0;

int cont_caindo = 0;
int cont_subindo = 0;
int cont_lento = 0;
int cont_transicao = 0;

float smoothedResultante   = 0;    // smoothed result
float samples              = 4;    // amount of samples

int stepFd = 0;

int colorPos = 1;

int isMoving = 0;

int CONST_PARADO = 1;
int CONST_MOVENDO = 2;

int estadoAtual = 1; //inicia com estado atual PARADO

float CONST_THRESHOLD_MOVIMENTACAO = 0.07; //threshold para deteccao de movimentacao - 0 eh PARADO 
int CONST_TEMPO_PARADO = 1000;  //tempo para ocnsiderar que o movimento parou   
int CONST_TEMPO_MOVENDO = 5;   //tempo para considerar que o movimento comecou

float tempo_parado_total = 0;
float prov_tempo_parado_total = 0;

void setup(){
	Serial.begin(57600);  // Output do serial com Baud Rate de 57600
         
        //Seta o valor de referencia do Arduino de acordo com o 
        //valor vindo do acelerometro na sua saida VCC +- 3.3V 
        analogReference(EXTERNAL);

	redVal = 0; 
	greenVal = 0; 
	blueVal = 0;

	lastRedVal = 0;
	lastGreenVal = 0;
	lastBlueVal = 0;

	update();
}

/*
 * This function updates the LED outputs.
 */  
void update() {
	analogWrite(LedR, redVal);
	analogWrite(LedG, greenVal);
	analogWrite(LedB, blueVal);

}

/*
 *
 */
void updateRGB(int r, int g, int b) {
	analogWrite(LedR, r);
	analogWrite(LedG, g);
	analogWrite(LedB, b);

	lastRedVal = r;
	lastGreenVal = g;
	lastBlueVal = b;

}

/*
 *
 */  
void color_random( float qtMov ) 
{
	qtMov = constrain(qtMov, 0, 9.0);

	h = h + (qtMov*10);

	if (h > 360.0)  h -= 360.0;


	if ((h > 0) && (h < 60 )) {

		//Orange  (RGB: 255, 127, 0)
		redVal = 0;
		greenVal = 240;
		blueVal = 255;

	} else if ((h > 60) && (h < 120 )) {

		//Yellow  (RGB: 255, 255, 0)
		redVal = 0;
		greenVal = 210;
		blueVal = 255;


	} else if ((h > 120) && (h < 180 )) {

		//Indigo  (RGB: 111, 0, 255)
		redVal = 144;
		greenVal = 255;
		blueVal = 20;


	} else if ((h > 180) && (h < 240 )) {

		//Violet  (RGB: 143, 0, 255)
		redVal = 112;
		greenVal = 255;
		blueVal = 20;

	}  else if ((h > 240) && (h < 300 )) {

		//green  (RGB: 0, 255, 0)
		redVal = 255;
		greenVal = 0;
		blueVal = 255; 

        }  else if ((h > 300) && (h < 360 )) {

		//blue  (RGB: 0, 0, 255)
		redVal = 255;
		greenVal = 255;
		blueVal = 0; 
        }

	update();

}

/*
 *
 */
void color_cicle( int posC ) 
{
        
        //colorPos = posC;

        if (colorPos>6)
          colorPos = 1;

	if (colorPos == 1) {

		//Orange  (RGB: 255, 127, 0)
                redVal = 255;
		greenVal = 100;
		blueVal = 0;


	} else if (colorPos == 2) {

		//Yellow  (RGB: 255, 255, 0)
		redVal = 255;
		greenVal = 200;
		blueVal = 0;


	} else if (colorPos == 3) {

		//Indigo  (RGB: 111, 0, 255)
		redVal = 111;
		greenVal = 0;
		blueVal = 255;


	} else if (colorPos == 4) {

		//Violet  (RGB: 143, 0, 255)
		redVal = 143;
		greenVal = 0;
		blueVal = 255;

	}  else if (colorPos == 5) {

		//green  (RGB: 0, 255, 0)
		redVal = 0;
		greenVal = 255;
		blueVal = 0;

        }  else if (colorPos == 6) {

		//blue  (RGB: 0, 0, 255)
		redVal = 0;
		greenVal = 0;
		blueVal = 255;
        }

	update();

}

/* 
 * This function updates one of the color variables
 * either getting brighter or getting dimmer.
 * It also updates the outputs 
 */
void color_transition( float qtMov )
{

	qtMov = constrain(qtMov, 0, 9.0);

	h = h + qtMov;

	if (h > 360.0)  h -= 360.0;

	hsv_to_rgb(h, s, v, &redVal, &greenVal, &blueVal);
	update();

}

/*
 *
 */
void color_change(float fr, float fg, float fb) {

	if ( fr > 2.00 )  {
		lastRedVal =  map( constrain(fr, 0, 3.3), 0, 3.3, 0, 255 ); 

	}

	if ( fg > 2.00  )  {
		lastGreenVal = map( constrain(fg, 0, 3.3), 0, 3.3, 0, 255 ); 

	}

	if ( fb > 2.00 ) {
		lastBlueVal = map( constrain(fb, 0, 3.3), 0, 3.3, 0, 255 ); 

	}

	updateRGB(lastRedVal, lastGreenVal, lastBlueVal);
	delay(10);

}

/*
 *
 */
void hsv_to_rgb(float h, float s, float v, 
		unsigned char *rc, unsigned char *gc, unsigned char *bc) {

	int h_i = ((int)(h/60)) % 6;

	float f = (h/60) - (int)(h/60);

	float r,g,b;

	float p = v * (1.0 - s);
	float q = v * (1.0 - f*s);
	float t = (1.0 - (1.0 - f)*s);

	switch(h_i) {
	case 0:  r = v; g = t; b = p; break;
	case 1:  r = q; g = v; b = p; break;
	case 2:  r = p; g = v; b = t; break;
	case 3:  r = p; g = q; b = v; break;
	case 4:  r = t; g = p; b = v; break;
	case 5:  r = v; g = p; b = q; break;
	}

	*rc = red_max - (char)((red_max - red_min)*r);
	*gc = green_max - (char)((green_max - green_min)*g);
	*bc = blue_max - (char)((blue_max - blue_min)*b);
}

/*
 *
 */
void update_pulso() {

	if (pulso > 255) {
		sig_pulso = 1;
	} else if (pulso < 0) {
		sig_pulso = 0;
	}

	if (sig_pulso == 1) {
		pulso -=0.2;
	} else if (sig_pulso == 0) {
		pulso +=0.2;  
	}

}

/*
 * Realiza um fade da ultima cor ateh a cor do pulso ou black
 */
void fade_to_pulse(int stepFade) {
       
  
        if (redVal > 0) {
           redVal = redVal - (5 * (255/redVal));
        }
       
        if (greenVal > 0) {
           greenVal = greenVal - (5 * (255/greenVal));
        }
           
        if (blueVal > 0) {  
           blueVal = blueVal - (5 * (255/blueVal)); 
        }
       
        
        constrain(redVal, 0, 255);
        constrain(greenVal, 0, 255);
        constrain(blueVal, 0, 255);
        
        update();
}

void loop(){

	oldValX = forcaX;
	oldValY = forcaY;
	oldValZ = forcaZ;

	oldResultante = resultante;

	//Leitura Bruta dos valores de aceleracao do sensor
	valX = analogRead(pinX);    // Ler o pino X
	valY = analogRead(pinY);    // Ler o pino Y
	valZ = analogRead(pinZ);    // Ler o pino Z

        //Transforma a leitura de entrada de 0 a 1023 na leitura de 
        //tensao dereferencia 3.3 vindo do acelerometro
        tensaoX = (valX) * (3.2) / (1023);
        tensaoY = (valY) * (3.2) / (1023);
        tensaoZ = (valZ) * (3.2) / (1023);

	//calcula os valores de forca aplicados aos eixos
	forcaX = tensaoX-1.65;
	forcaY = tensaoY-1.65;
	forcaZ = tensaoZ-1.65;

	//Calcula a diferenca de aceleracao dos eiXos
	diffX = forcaX-oldValX;
	diffY = forcaY-oldValY;
	diffZ = forcaZ-oldValZ;

	if (diffX<0) diffX *= (-1);
	if (diffY<0) diffY *= (-1);
	if (diffZ<0) diffZ *= (-1);

        resultante = sqrt( sq(forcaX) + sq(forcaY) + sq(forcaZ) );

	qtMovimentacao = diffX + diffY + diffZ;

	//diffResultante = oldResultante - resultante;

        //Suavisa a amostra da resultante
        smoothedResultante = smoothedResultante + ((resultante - smoothedResultante)/samples);
        
        diffResultante = smoothedResultante - 0.84;

	
        //  DEBUG DA TENSAO DE ENTRADA - PARA AVALIAR A MONTAGEM
        //COM O USO DE UM MULTIMETRO
        /*
        Serial.print(" - tensaoX:");
        Serial.print( tensaoX );
        Serial.print(" tensaoY:");  
        Serial.print( tensaoY);
        Serial.print(" tensaoZ:");
        Serial.print( tensaoZ );
        */
      
      	/*
        Serial.print(" - forcaX:");
        Serial.print(forcaX);
        Serial.print(" forcaY:");  
        Serial.print(forcaY);
        Serial.print(" forcaZ:");
        Serial.print(forcaZ);
      	 */
      
      	/*
        Serial.print(" - DiffX:");
        Serial.print(diffX);
        Serial.print(" DiffY:");  
        Serial.print(diffY);
        Serial.print(" DiffZ:");
        Serial.print(diffZ);
      	 */

	//Serial.print(" - resultante: ");
	//Serial.print(resultante);

        /*
	Serial.print(" - diffResultante: ");
	Serial.print(diffResultante);
        */
        
        /*
	Serial.print(" - pulso: ");
	Serial.print(pulso);
        */
 
        /*
	Serial.print(" - qtMovimentacao: ");
	Serial.print(qtMovimentacao);
        */
        
        /*
        Serial.print(" - stepFd: ");
        Serial.print(stepFd);
        */

        //Serial.print(" - ### tempo_parado_total: ");
        //Serial.print(tempo_parado_total);

        if (
             (qtMovimentacao > CONST_THRESHOLD_MOVIMENTACAO) 
           ) { 
                
                //Define o tempo de espera ao retorno do estado parado
                prov_tempo_parado_total = CONST_TEMPO_PARADO * (qtMovimentacao);
   
   
                if (prov_tempo_parado_total > tempo_parado_total) {
                  tempo_parado_total = prov_tempo_parado_total;
                }
             
		cont_transicao++;
		if ( cont_transicao > CONST_TEMPO_MOVENDO ) {
  
                    estadoAtual = CONST_MOVENDO;
                    cont_transicao = 0;
                    cont_lento = 0;
 
			pulso = 0;
                        stepFd = 255;
		} 


	} else { 
		
  		cont_lento++;
		if ( cont_lento >  tempo_parado_total ) {
                      
                    Serial.print(" - ### tempo_parado_total utilizado: ");
                    Serial.println(tempo_parado_total);
  
                    estadoAtual = CONST_PARADO;
	            cont_transicao = 0;
                    cont_lento = 0;
                    stepFd = 0;
                    
                    tempo_parado_total = 10;

		}

	} 

        if (estadoAtual == CONST_PARADO) {
          //Serial.print(" - ### lento ");
          updateRGB(constrain(pulso, 0, 255),0,0);
          //updateRGB(255,0,0);
          isMoving = 1;
          
        } else if (estadoAtual == CONST_MOVENDO) {         
          //Serial.print(" - ### movendo ");
          if(isMoving == 1) {
		color_cicle(colorPos++);
                isMoving = 0;
          }
          
        }


        /*
        Debug de LEDs
        redVal = 0; 
	greenVal = 0; 
	blueVal = 255;        
        update();
        */

	update_pulso();

	//Serial.println(" ");

}

